package com.lejos.carcontrol.version1;


import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;

public class Car {
  public static final int DEF_SPEED = 360;
  private byte LEN = 45;
  private UltrasonicSensor sonar;
  
  public Car(){
    sonar = new UltrasonicSensor(SensorPort.S3);
  }
  public int getDistance(){
    sonar.ping();
    Sound.pause(30);
    return sonar.getDistance();
  }
  public void Forard(int speed){
    int s = (int)(1.0*speed/LEN * DEF_SPEED);
    Motor.A.setSpeed(s);
    Motor.A.backward();
    Motor.C.setSpeed(s);
    Motor.C.backward();
  }
  
  public void Back(int speed){
    int s = (int)(1.0*speed/LEN * DEF_SPEED);
    Motor.A.setSpeed(s);
    Motor.A.forward();
    Motor.C.setSpeed(s);
    Motor.C.forward();
  }
  
  public void turnLeft(int speed){
    int s = (int)(1.0*speed/LEN * DEF_SPEED);
    Motor.C.stop();
    //Motor.C.setSpeed(s);
    //Motor.C.backward();
    Motor.A.rotate(-750);
    Forard(LEN);
  }
  
  public void turnRight(int speed){
    int s = (int)(1.0*speed/LEN * DEF_SPEED);
    //Motor.A.setSpeed(s);
    //Motor.A.backward();
    Motor.A.stop();
    Motor.C.rotate(-750);
    Forard(LEN);
  }
  
  public void Stop(){
    Motor.A.stop();
    Motor.C.stop();
  }
  
  public void sensorRotate(int angle){
    Motor.B.rotate((int)(angle*57.0/8));
  }
  /*static class SensorContral extends Thread{
    
    @Override
    public void run() {
      // TODO Auto-generated method stub
      while(true){
        Motor.B.rotate(-160 * 8);
        if(!Constant.isMoving){
          Motor.B.stop();
          while(!Constant.isMoving);
        }
        Motor.B.rotate(160 * 8);
        if(!Constant.isMoving){
          Motor.B.stop();
          while(!Constant.isMoving);
        }
      }
    }
    
  }*/
}
